/*
 *  td.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos García Orden on 12/07/2010
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "d.h"

D::D()
:	Component(),
	lam(),
	gam(),
	//s(),
	theta(),
	partial_qdotOld(),
	//betac(),
	// c(),
	c0(),
	lam0(),
	c10(),
	c11(),
	eta0(),
	//a(),
	mu0(),
	mu1(),
	beta0(),
	pLam(),
	pGam(),
	// pS(),
	fT(),
	Q(),
	// h(),
	denom(0.0),
	toldenom(1.0e-6),
	//f_eta0(),
	mode()
{}

// ***

D::D(vector<string>& vp, vector<double>& param, double &ma, int di)
:	Component(vp,di),
	mass(ma),
	lam(),
	gam(0.0),
	theta(300.0),
	partial_qdotOld(0.0),
	// betac(4.0),
	// c(10.0),
	c0(1.0),	// With 10 oscillates more
	lam0(1),
	c10(100.0),
	c11(0.5),
	eta0(100.0),
	// a(10.0),
	mu0(5.0),
	mu1(0.1),
	denom(0.0),
	toldenom(1.0e-6),
	mode("log"),
	eta_ten(0.0),
	eta_com(100.0)
	//f_eta0(),
	//f_c10(),
	//f_mu0()
{

	c10 =	param[0];
	c11 =	param[1];
	// betac = param[2];
	c0 =	param[3];
	mu0 =	param[4];
	mu1 =	param[5];
	eta0 =	param[6];
	//a =	param[7];
	//c =	param[8];
	
	pLam = lam;
	pGam = gam;
	// pS = s;
	
	if (c10!=0.0) beta0 = 2.0*mu0/c10; 
	else beta0 = 0.0;
		
//	numDofs=2*dim+1;
//	cout << "numDofs = " << numDofs << endl;	
}

D::D(vector<string>& vp, vector<double>& param, map<string,string>& param2, double &ma, int di)
:	Component(vp,di),
	mass(ma),
	lam(),
	gam(0.0),
	//theta(300.0),
	partial_qdotOld(0.0),
	//betac(4.0),
	//c(10.0),
	c0(1.0),	// With 10 oscillates more
	lam0(1),
	c10(100.0),
	c11(0.5),
	eta0(100.0),
	//a(10.0),
	mu0(5.0),
	mu1(0.1),
	denom(0.0),
	toldenom(1.0e-6),
	eta_ten(0.0),
	eta_com(200.0), 
	mode("log")
{

	c10 =	param[0];
	c11 =	param[1];
	//betac = param[2];
	c0 =	param[3];
	mu0 =	param[4];
	mu1 =	param[5];
	eta0 =	param[6];
	//a =	param[7];
	//c =	param[8];
	
	pLam = lam;
	pGam = gam;
	//pS = s;
	
	if (c10!=0.0) beta0 = 2.0*mu0/c10; 
	else beta0 = 0.0;

	


	stringstream stnum;

 	if (param2.count("mode")>0)
      	{	
		mode = param2.find("mode")->second;
		//cout << "mode " << mode << endl;
	}

	stringstream stnum1;
    	if (param2.count("eta_ten")>0)
	{
		stnum1 << param2.find("eta_ten")->second;
		stnum1 >> eta_ten;
	}

	stringstream stnum2;
	if (param2.count("eta_com")>0)
	{
		stnum2 << param2.find("eta_com")->second;
		stnum2 >> eta_com;

	}
			
//	numDofs=2*dim+1;
//	cout << "numDofs = " << numDofs << endl;	
}

// ***

void D::calcFi(const vector<double>& qddot)
{
	if (mass!=0.0)
	{
		int i;
		int j;
		
		for(i=0; i<numDofs; ++i)
		{
			Fi[i] = 0.0;
			for(j=0; j<numDofs; ++j)	
			{
				if(integratorType==0)  Fi[i] += M(i,j) *qddot[ localGlob[j] ];
				else	Fi[i] += M(i,j) * (dq[j] - dqC[j]);
			}
		}			
	}
}

// ***

void D::calcFq(const int& nt)
{
	
	lam = calcLam(q);
	// s = q[numDofs-1];
	
	if (integratorType==0)	// Standard integrator
	{	
		gam = calcGam(pGam,lam, deltaT);
	
		fT = partialVLam(lam, gam);	// total force
		Q = -partialVGam(lam, gam);	// viscous force
		// theta = partialVS(lam, gam);	// temperature
		
		//h = c*(theta - thetab);	// heat flux
	
		for (i=0; i<dim; ++i)
		{
			Fq[i] = (fT/lam)*(q[i]-q[i+dim]);
			Fq[i+dim] = -Fq[i];
			//cout << Fq[i] << endl;
		}
		
		// entropy		
		//Fq[numDofs-1] = dq[numDofs-1] - ( (Q*Q)/Eta(theta) - h ) / theta;
/*		
		cout << "lam = "<< lam << endl;
		cout << "gam = "<< gam << endl;
		cout << "s = "<< s << endl;
		cout << "theta = "<< theta << endl;
		cout << "fT = "<< fT << endl;
		cout << "Q = "<< Q << endl;
		cout << "h = "<< h << endl;
*/
//		cout << "Fq[numDofs-1] = " << dq[numDofs-1] << endl;
	}
	else if (integratorType==1 || integratorType==2 ) // Midpoint and consistent
	{		
		if(qMid.size()==0)
		{
			qMid.resize(q.size());
			dqMid.resize(dq.size());
		}
		
		for(i=0; i<qMid.size();i++)
		{
			qMid[i]=0.5*(q[i]+qC[i]);
			dqMid[i]=0.5*(dq[i]+dqC[i]);
		}
		
		lamMid = calcLam(qMid);				
		if (integratorType==1)
		{
			// sMid = qMid[numDofs-1];	// Midpoint

			gam = calcGam(pGam, lamMid, deltaT);
		
			gamMid = 0.5*(gam + pGam);

			fTMid = partialVLam(lamMid,gamMid);	// total force
			QMid = -partialVGam(lamMid,gamMid);	// viscous force
			// thetaMid = partialVS(lamMid,gamMid);	// temperature
		}
		else	// Consistent
		{
			lamMid = calcLam(qMid);				
			
			//gam = calcGam(pGam, lam, deltaT);
			gam = pGam;
			
			fTMid = partialVLam(lam,gam);	// total force
			QMid = -partialVGam(lam,gam);	// viscous force
			// thetaMid = partialVS(lam,gam,s);// temperature			
		}
		
		// hMid = c*(thetaMid - thetab);	// heat flux
		
		for (i=0; i<dim; ++i)
		{
			Fq[i] = (fTMid/lamMid)*(qMid[i]-qMid[i+dim]);
			Fq[i+dim] = -Fq[i];
		}
		
		// Fq[numDofs-1] = dqMid[numDofs-1] - ( (QMid*QMid)/Eta(thetaMid) - hMid ) / thetaMid;
	}
/*
	for (i=0; i<numDofs; ++i)
	{
		cout << "Fq[" << i << "] = " << Fq[i] << endl;
	}
*/
}

// ***

void D::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***

void D::calcKi(double & partial_qddot)
{
	if (mass!=0.0)
	{
		if(partial_qddot!=partial_qddotOld)	// In order to compute Ki just once
		{
//			int i;
//			int j;
//			for(i=0; i<numDofs; i++)
//				for(j=0; j<numDofs; j++) Ki[i][j] = M(i,j)*partial_qddot;
			 Ki = M * partial_qddot;
		}	
		partial_qddotOld = partial_qddot;		
	}
}

// ***

void D::calcKq(int nt)
{
	calcLocalKqNumeric();	// Analytical tangent, TO BE DONE
}

// ***

void D::calcKdq(double & partial_qdot)
{
	if (integratorType!=0) partial_qdot/= 2.0;	// Midpoint and consistent
	if(partial_qdot!=partial_qdotOld)	// In order to compute Kdq just once
	{
		Kdq(numDofs-1,numDofs-1) = partial_qdot;
	}
	partial_qdotOld = partial_qdot;
}

// ***

void D::calcM(const vector<double>& q, const double& t)
{
	if (mass!=0.0)
	{
		int i;
		if(M.size()==0)
		{	
			M.setZero(numDofs,numDofs);
		}
		// Consistent mass matrix
		if(numDofs==6)	// dim=3
		{
			M(0,0) = mass/3.;
			M(0,3) = mass/6.; 
			M(1,1) = mass/3.; 
			M(1,4) = mass/6.;
			M(2,2) = mass/3.;
			M(2,5) = mass/6.;
			M(3,0) = M(0,3);
			M(3,3) = mass/3.;
			M(4,1) = M(1,4); 
			M(4,4) = mass/3.;
			M(5,2) = M(2,5);
			M(5,5) = mass/3.;	
		} 
		else if(numDofs==4)	// dim=2
		{
			M(0,0) = mass/3.;
			M(0,2) = mass/6.;
			M(1,1) = mass/3.;
			M(1,3) = mass/6.;
			M(2,0) = M(0,2);
			M(2,2) = mass/3.;
			M(3,1) = M(1,3);
			M(3,3) = mass/3.;
		} 
		else			// dim=1
		{
			M(0,0) = mass/3.;
			M(0,1) = mass/6.;
			M(1,0) = M(0,1);
			M(1,1) = mass/3.;
		}			
	}
}

// ***

void D::calcMLumped(const vector<double>& q, const double& t)
{
	if (mass!=0.0)
	{
		if (MLumped.size()==0)
		{
			MLumped.resize(numDofs);
		}
		
		int i;
		for (i=0; i<numDofs; ++i)	MLumped[i] = mass/2.0;		
	}
}

// ***

void D::setDofs(void)
{
/*
	map<string,Point*,less<string> >::iterator it;
	for(it=mPoints.begin(); it!=mPoints.end(); it++)
	{
		for(int j=0; j<dim; j++)
		{
			(*it).second->setDof(0,j);
		}
	}
	// No inertia dofs added
*/	
	
	// It must not add any dof, but must set variable numDofs
	numDofs=2*dim+1;

	dofT.set(0,1);	// Component dof added at position 0
}

// ***

void D::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{	
	/*
	cout << "numDofs = " << numDofs << endl;
	cout << "localGlob is " << endl;
	printSTLVector(localGlob);
	*/
	int p;

	q.resize(numDofs);
	
	for(p=0; p<pointNames.size(); ++p)
	{		
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[p*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
	}
	auxd = 0.0;
	for (i=0; i<dim; ++i)
	{
		auxd+= (q_in[ localGlob[i] ]-q_in[ localGlob[i+dim] ])*(q_in[ localGlob[i] ]-q_in[ localGlob[i+dim] ]);
	}
	lam = sqrt(auxd);
	lam0 = lam;
	// s = entropy(lam, gam, theta);
	
	// q_in[ localGlob[numDofs-1] ] = s;		// Default, initial entropy
/*	
	for(i=0; i<q_in.size(); ++i)	cout << " " << q_in[i];
	cout << endl;
*/	
	pLam = lam;
	pGam = gam;
	// pS = s;	
/*
	cout << "initial lam = " << lam << endl;
	cout << "initial gam = " << gam << endl;
	cout << "initial theta = " << theta << endl;
	cout << "initial s = " << s << endl;
*/

	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
}

// ***

void D::setMatAtComp(Material* ma)
{}

// ***

void D::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	Fq.setZero(numDofs);
	if(grav!=0.0)
	{
		int i;
		int j;		
		Ft.resize(numDofs);
		// Build Ft once for all  Ft= mass * g
		Ft[gravDir-1] = - mass * grav / 2.0;
		Ft[dim+gravDir-1] = - mass * grav / 2.0;
	}
	
	if(st!=0 && mass!=0.0)
	{
		Fi.resize(numDofs);
		M.setZero(numDofs,numDofs);
	}	
}

// ***

//void TD::initializeCompTang(const bool& st, const bool& pf)
void D::initializeCompTang(const bool& st)
{
	Kq.setZero(numDofs,numDofs);

	if(st!=0)
	{
		if (mass!=0.0)
		{
//			Ki.resize(numDofs);
//			for(i=0; i<numDofs;++i)	Ki[i].resize(numDofs);
			Ki.setZero(numDofs,numDofs);
		}
//		Kdq.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Kdq[i].resize(numDofs);
		Kdq.setZero(numDofs,numDofs);
	}
}

// ***

void D::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	// Kinetic
	if(dq.size()!=0 && (M.size()!=0 || MLumped.size()!=0) )
	{
		int j;
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofs; ++i)
		{
			if (M.size()!=0)	// Consistent
			{
				for (j=0; j<numDofs; ++j)
				{
					Ekin+= 0.5 * dq[i] * M(i,j) * dq[j];
				}
			}
			else	// Lumped
			{
				Ekin+= 0.5 * dq[i] * MLumped[i] * dq[i];
			}						
		}
	}
	
	Epot+= V(lam,gam);

	if (integratorType==0)	// Trapezoidal.  Other integrators TO BE DONE
	{
		// Eamb+= c*( 0.5*(partialVS(lam,gam)+partialVS(pLam, pGam)) - thetab)*deltaT;	// heat flux
	}
	else if (integratorType==1)	// Midpoint
	{
		// Eamb+= c*( partialVS(lamMid, gamMid, sMid) - thetab)*deltaT;
	}
	else if (integratorType==2)	// Consistent
	{
		// Eamb+= c*( partialVS(lam, gam, s) - thetab)*deltaT;
	}
}

// ***

void D::calcEntropy(double& Entropy, double& EntropyAmb)
{
	/*Entropy+= s;

	if (integratorType==0)	// Trapezoidal.  Other integrators TO BE DONE
	{
		EntropyAmb+= c*( 0.5*(partialVS(lam,gam,s)+partialVS(pLam, pGam, pS)) - thetab )*deltaT/thetab;
	}
	else if (integratorType==1)	// Midpoint
	{
		EntropyAmb+= c*( partialVS(lamMid,gamMid,sMid)- thetab )*deltaT/thetab;
	}
	else if (integratorType==2)	// Consistent
	{
		EntropyAmb+= c*( partialVS(lam,gam,s)- thetab )*deltaT/thetab;
	}	
	*/
}

// ***

void D::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***

double D::V(double lam, double gam)
{
	
	if (!mode.compare("log"))
	{
	t3 = log(lam / lam0);
	t4 = t3 *t3;
	t7 = gam *gam;
	t9 = beta0 * c10;
	return t12 = c10 * t4 / 0.2e1 + mu0 * t7 - t9 * t3 / lam * gam + t9 * t4 / 0.2e1;
	}

	if (!mode.compare("lin"))
	{
		t1 = lam - lam0;
		t2 = t1 * t1;
		t5 = gam * gam;
		t7 = beta0 * c10;

		t12 = c10 * t2 / 0.2e1 + mu0 * t5 - t7 * t1 * gam + t7 * t2 / 0.2e1;

		// cout << "t12 " << t12 << "\t t1 " << t1 << "\t t5 " << t5<< endl;

		return t12;
	}

	
}

// ***

double D::dlamV(double lam, double gam)
{
	if (!mode.compare("log"))
	{	
		t3 = log(lam / lam0);
		t5 = 0.1e1 / lam;
		t7 = beta0 * c10;
		t8 = lam *lam;
		t9 = 0.1e1 / t8;
		return t17 = c10 * t3 * t5 - t7 * t9 * gam + t7 * t3 * t9 * gam + t7 * t3 * t5;	
	}

	if (!mode.compare("lin"))
	{
		t1 = lam - lam0;
		t3 = beta0 * c10;
		t6 = c10 * t1 - t3 * gam + t3 * t1;
		// cout << "dlamV " << t6 << endl;
		return t6 = c10 * t1 - t3 * gam + t3 * t1;
	}
}

// ***

double D::dgamV(double lam, double gam)
{
	if (!mode.compare("log"))
	{
		t6 = log(lam / lam0);
		return t10 = (2 * mu0 * gam) - beta0 * c10 * t6 / lam;
	}

	if (!mode.compare("lin"))
	{
		return t6 = 2 * mu0 * gam - beta0 * c10 * (lam - lam0);
	}
}

// ***

double D::entropy(double lam, double gam)
{
	/*if (!mode.compare("log"))
	{
	}

	if (!mode.compare("lin"))
	{
		t1 = lam - lam0;
		t2 = t1 *t1;
		t4 = lam0 *lam0;
		t5 = 0.1e1 / t4;
		t7 = c11 * t2 * t5 / 0.2e1;
		t9 = 0.1e1 / lam0;
		t10 = betac * t1 * t9;
		t13 = log(theta / thetab);
		t14 = c0 * t13;
		t15 = gam * gam;
		//return t7 + t10 + t14 + mu1 * t15 + beta0 * (-c11 * t1 * t5 - betac * t9) * gam - beta0 * (-t7 - t10 - t14);
		return 0;
	}*/
}

// ***

double D::dentV(double lam, double gam)
{
	if (!mode.compare("log"))
	{
	}

	if (!mode.compare("lin"))
	{
	}
	
}

// ***

double D::eta(double lam, double gam)
{
	setEta();
	return eta0;

	if (!mode.compare("log"))
	{
	}

	if (!mode.compare("lin"))
	{
	}
}

// ***

double D::Eta()
{
	setEta();
	return eta0;
}

// ***

double D::integrateGam1ex(double& inigam, double& Lam, double& dt)
{
	// Explicit Euler
	return inigam - dt*partialVGam(pLam, pGam)/eta(pLam,pGam);
}

// ***

double D::integrateGam1im(double& inigam, double& Lam, double& dt)
{
	// Implicit Euler
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
//	double perturbation = 1.0e-7;

	gam = inigam;
	res = 1.0;
		
	while (fabs(res)>tolres)
	{
		res = gam - pGam + dt*partialVGam(Lam, gam)/eta(Lam,gam);
		resM = (gam-perturbation) - pGam + dt*partialVGam(Lam, gam-perturbation)/eta(Lam,gam-perturbation);
		resP = (gam+perturbation) - pGam + dt*partialVGam(Lam, gam+perturbation)/eta(Lam,gam+perturbation);
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	return gam;
}

// ***

double D::integrateGamMid(double& inigam, double& Lam, double& dt)
{
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
	double temp, tempM, tempP;

	gam = inigam;
	res = 2.0*tolres;	

	// Midpoint

	double gamMid;		
	while (fabs(res)>tolres)
	{
		if (integratorType!=2) gamMid = 0.5*(gam+pGam);
		else gamMid = gam;
		
		temp = partialVS(Lam,gamMid);
		res = gam - pGam + dt*partialVGam(Lam, gamMid)/Eta();
		tempM = partialVS(Lam,gamMid-perturbation);
		tempP = partialVS(Lam,gamMid+perturbation);
		resM = (gam-perturbation) - pGam + dt*partialVGam(Lam, gamMid-perturbation)/Eta();
		resP = (gam+perturbation) - pGam + dt*partialVGam(Lam, gamMid+perturbation)/Eta();
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
		// cout << tolres << "\t eta = " << eta0 << endl;
		//cout << "temp " << temp << endl;
		//cout << "res " << res << endl;
		//cout << "tempM " << tempM << endl;
		//cout << "tempP " << tempP << endl;
		//cout << "resM " << resM << endl;
		//cout << "resP " << resP << endl;
		//cout << "kGam " << kGam << endl;
		//cout << "gam " << gam << endl;
		//cout << "pGam " << pGam << endl;

	}
	
	return gam;
}

// ***
/*
double TD::integrateGamCon(double& inigam, double& Lam, double& S, double& dt)
{
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
	double temp, tempM, tempP;
	
	gam = inigam;
	res = 2.0*tolres;	
	
	// Consistent
	
	while (fabs(res)>tolres)
	{
		temp = partialVS(Lam,gam,S);
		res = gam - pGam + dt*partialVGam(Lam, gam, S)/Eta(temp);
		tempM = partialVS(Lam,gam-perturbation,S);
		tempP = partialVS(Lam,gam+perturbation,S);
		resM = (gam-perturbation) - pGam + dt*partialVGam(Lam, gam-perturbation, S)/Eta(tempM);
		resP = (gam+perturbation) - pGam + dt*partialVGam(Lam, gam+perturbation, S)/Eta(tempP);
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	
	return gam;
}
*/
// ***

double D::integrateGamTra(double& inigam, double& Lam, double& dt)
{
	double res;
	double resM;
	double resP;
	double tolres = 1.0e-6;
	double kGam;
	
	gam = inigam;
	res = 2.0*tolres;	
	
	// Trapezoidal rule
	
	while (fabs(res)>tolres)
	{
		res = gam - pGam + dt*0.5*( partialVGam(pLam, pGam)/eta(pLam,pGam) + partialVGam(Lam, gam)/eta(Lam,gam) );
		resM = (gam-perturbation) - pGam + dt*0.5*( partialVGam(pLam, pGam)/eta(pLam,pGam) + partialVGam(Lam, gam-perturbation)/eta(Lam,gam-perturbation) );
		resP = (gam+perturbation) - pGam + dt*0.5*( partialVGam(pLam, pGam)/eta(pLam,pGam) + partialVGam(Lam, gam+perturbation)/eta(Lam,gam+perturbation) );
		kGam = (resP-resM)/(2.0*perturbation);
		gam-= res/kGam;
	}
	
	return gam;
}

// ***

void D::updateComp(void)
{
	pLam = lam;
	pGam = gam;
/*
	cout << "Pillado " << endl;
	cout << "pLam = " << pLam << endl;
	cout << "pGam = " << pGam << endl;
	cout << "pS = " << pS << endl;
*/
}

// ***

double D::calcLam(vector<double>& v)
{
	auxd = 0.0;
	for (i=0; i<dim; ++i)
	{
		auxd+= (v[i]-v[i+dim])*(v[i]-v[i+dim]);
	}
	return sqrt(auxd);	
}

// ***

void D::setInitialConditions(vector<double>& q_in, vector<double>& dq_in)
{
	// Initial condition in temperature must be transformed in entropy
	vector<double> pos(2*dim);
	int i;
	double auxLam;
	double auxTheta;
	double auxS;
	double auxQ;
	double auxH;

	for (i=0; i<2*dim; ++i)	pos[i] = q_in[localGlob[i]];
	auxLam = calcLam(pos);
	auxS = entropy(auxLam, 0.0);
	// q_in[ localGlob[numDofs-1] ] = auxS;
	
	// Initial entropy velocity is computed from the initial conditions
	auxQ = -partialVGam(auxLam,0.0);	// viscous force
	// auxH = c*(theta - thetab);	// heat flux
	// dq_in[ localGlob[numDofs-1] ] = ( (auxQ*auxQ)/Eta() - auxH );

/*
	cout << "lamini = " << auxLam << endl;
	cout << "theta = " << theta << endl;
	cout << "sini = " << auxS << endl;
	cout << "dsini = " << dq_in[ localGlob[numDofs-1] ] << endl;
	cout << "lcg " << localGlob[numDofs-1] << endl; 
*/
}

// ***

double D::getTemperature(void)
{
	return dentV(lam,gam);
}

// ***

double D::calcGam(double& gamini, double& Lam, double& dt)
{
	if (integratorType==0)
	{
		return integrateGamTra(gamini, Lam, dt);	// Trapezoidal.  Other integrators TO BE DONE
	}
	else if (integratorType==1 || integratorType==2)
	{
		return integrateGamMid(gamini, Lam, dt);	// Midpoint	and consistent
	}
	
//	return integrateGam1ex(gamini,Lam,S,dt);	// Explicit Euler
//	return integrateGam1im(gamini,Lam,S,dt);	// Implicit Euler
}

// ***

void D::setIniDofs(vector<double>& k)
{
	theta = k[0];
}

// ***

void D::calcLocalKqNumeric()
{
	int j;
 
	if(Fq.size()!=0)
	{
		if(Kq.size()==0)
		{
		  Kq.setZero(numDofs,numDofs);
		}
 
		for (j=0;j<numDofs; ++j)
		{
			q[j]+= perturbation;
			calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
		        Kq.col(j) = Fq;
 
			q[j]-= 2.0*perturbation;
			calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
			Kq.col(j) -= Fq;      
 
			q[j]+= perturbation;
		}
		Kq /= (2.0*perturbation);
		calcFq(1);      // 1 means no simultaneous tangent computation (if available)
 
 // cout << "Kq es " << endl;
 // printSTLMatrix(Kq);
 
	}       
 }  
// ***

double D::DlamV(double lam, double gam)
{
	denom = 2.0*(lam-pLam);
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom )
	{
		return ( V(lam,gam)-V(pLam,gam)+V(lam,pGam)-V(pLam,pGam) )/denom;
	}
	else 
	{
		return ( dlamV(pLam,gam) + dlamV(pLam,pGam) )/2.0;
	}

	
}	

// ***

double D::DgamV(double lam, double gam)
{
	denom = 2.0*(gam-pGam);
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom)
	{
		return ( V(pLam,gam)-V(pLam,pGam)+V(lam,gam)-V(lam,pGam) )/denom;
	}
	else
	{
		return ( dgamV(pLam,pGam) + dgamV(lam,pGam) )/2.0;
	}
}	

// ***

double D::DentV(double lam, double gam)
{
	/*denom = 2.0*(s-pS);
//	if (denom!=0.0)
	if ( fabs(denom)>toldenom)
	{
		return ( V(pLam,pGam)-V(pLam,pGam)+V(lam,gam)-V(lam,gam) )/denom;	
	}
	else
	{
		return ( dentV(pLam,pGam) + dentV(lam,gam) )/2.0;
	}
	*/
}	

// ***

double D::partialVLam(double lam, double gam)
{
	if (integratorType!=2)	return dlamV(lam, gam);	// Standard gradient
	else return DlamV(lam, gam);						// Discrete gradient
}

// ***

double D::partialVGam(double lam, double gam)
{
	if (integratorType!=2)	return dgamV(lam, gam);	// Standard gradient
	else return DgamV(lam, gam);						// Discrete gradient
}

// ***

double D::partialVS(double lam, double gam)
{
	if (integratorType!=2)	return dentV(lam, gam);	// Standard gradient
	else return DentV(lam, gam);						// Discrete gradient
}

// ***

void D::calcPhi(const vector<double>& q)
{}

// ***

void D::calcDPhi(const vector<double>& q)
{}

// ***

void D::calcMSmooth(const vector<double>& q_in, const double& t)
{
}

// ***

void D::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{

	//setEta();

	if(FSmooth.size() == 0) 
	{
		FSmooth.setZero(1,1);
//		FSmooth.resize(1);
//		FSmooth[0].resize(1);
	}


	  ofstream myfile;
	  myfile.open ("force.txt",ios::app);
	  myfile << Fq[3] << "\t" << eta0 << endl; 
	  myfile.close();
	  

}

void D::setEta()
{
	/*
	cout << dq[0] << endl;
	cout << dq[1] << endl;
	cout << dq[2] << endl;
	cout << dq[3] << endl;
	cout << dq[4] << endl;
	cout << dq[5] << endl;
	*/

	if (eta_ten != 0.0)
	{
		if (dq[3]-dq[0] >= 0)
		{
			eta0 = eta_ten;
		}
		else
		{
			eta0 = eta_com;
		}
	}
}

int D::checkState()
{
return 0;
}
